#include <Wire.h>

// 7bit addressing
#define GYROS_ADDR_INIT 0x53  //WM+ starts out deactivated at address 0x53
#define GYROS_ADDR 0x52   // Wii Motion Plus
#define DATA_SIZE 6

byte buff[DATA_SIZE]; //six data bytes
int gyroX0, gyroY0, gyroZ0; //calibration zeroes

int gyroX, gyroY, gyroZ; //calibration zeroes

  long aci_X = 0;

void setup(){
  Serial.begin(9600);
  setup_I2Csensors();
  
  pinMode(13,OUTPUT);
  pinMode(12,INPUT);
  digitalWrite(12,HIGH);
}  



void loop(){


 
while(1){
      readGyros();
      
      //if ((gyroX <40) && (gyroX >-40)) gyroX = 0;
      //aci_X += (gyroX/14); 
      
      
      Serial.println(gyroX);
      }
}


void setup_I2Csensors(){
   Wire.begin(); // join as master

   initGyros();
   delay(100);
   calibrateGyrosZeroes();
}

void initGyros(){
Wire.beginTransmission(GYROS_ADDR_INIT); //WM+ starts out deactivated at address 0x53
Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
   Wire.send(0x04);
   Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
   delay(10);
}

void readGyros(){
    gyrosSendZero(); //send zero before each request (same as nunchuck)
    Wire.beginTransmission(GYROS_ADDR);
    Wire.requestFrom(GYROS_ADDR, DATA_SIZE); //request the six bytes from the WM+
    if(Wire.available() == DATA_SIZE) {
        for (int i=0; i<DATA_SIZE; i++) buff[i] = Wire.receive();

        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
        //for info on what each byte represents
       gyroZ = ((buff[3] >> 2) << 8) + buff[0] - gyroZ0;
       gyroX = ((buff[4] >> 2) << 8) + buff[1] - gyroX0;
       gyroY = ((buff[5] >> 2) << 8) + buff[2] - gyroY0;
       
       if (digitalRead(12)==LOW)
         {
          gyroZ0 = ((buff[3] >> 2) << 8) + buff[0];
          gyroX0 = ((buff[4] >> 2) << 8) + buff[1];
          gyroY0 = ((buff[5] >> 2) << 8) + buff[2];
          aci_X = 0;
         }
        
    }else{
        Serial.println("ERR_readGyros");
    }
    Wire.endTransmission();
}

void calibrateGyrosZeroes(){
    long tempX=0, tempY=0, tempZ=0;
    gyroX0 = 0; gyroY0 = 0; gyroZ0 = 0;
    for (int i=0; i<10; i++){
        readGyros();
        delay(50);
        tempZ += gyroZ;
        tempX += gyroX;
        tempY += gyroY;
    }

    // average 10 readings
    gyroZ0 = tempZ / 10;
    gyroX0 = tempX / 10;
    gyroY0 = tempY / 10;
}

void gyrosSendZero(){
   Wire.beginTransmission(GYROS_ADDR);//now at address 0x52
   Wire.send(0x00); //send zero to signal we want info
   Wire.endTransmission();
}
